﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using System.Timers;
using System.Threading;
using System.IO.Ports;

namespace SCI_actuator
{
    public partial class DrawCurve : Form
    {
        private menu_control frmain;
        private GroupCommand group_command;

        private bool flag_ready1;
        private bool flag_ready2;

        private double l_a, l_b, l_c, l;    //机械臂关节长度
        private double l_x, l_x_min,r;              //r为半径
        private double degree_a, degree_b, degree_c;  //度为单位
        private double radian_a, radian_b, radian_c;  //弧度为单位
        private double radian_d;
        private int del_a, del_b, del_c;
        private byte a_h_byte, a_l_byte, b_h_byte, b_l_byte, c_h_byte, c_l_byte;
        private string a_h_string, a_l_string, b_h_string, b_l_string, c_h_string, c_l_string;
        private double degree_d_max;
        private int c_count, point_count;
        private int middle;

        private double degree_c_max;

        private System.Timers.Timer action1;
        private System.Timers.Timer action2;

        private void Draw_Init()
        {
            l_a = 750;     //0.1mm
            l_b = 675;
            l_c = 515;
            l = l_a + l_b + l_c;
            r = 440;                  //此时的半径为能够画出的最大半径
            l_x = l_a + l_b + l_c;    //初始值
            l_x_min = 1250;
            degree_d_max = Math.Asin(r / (l - r)) * 180 / Math.PI;   //c的最大值
            degree_c_max = 70;   //c能够转动的最大值
            c_count = 0;
            point_count = 0;

            degree_a = 0;
            degree_b = 0;
            degree_c = 0;
            radian_a = 0;
            radian_b = 0;
            radian_c = 0;
            
            middle = 556;
        }

        public DrawCurve(menu_control fr)
        {
            InitializeComponent();

            this.frmain = fr;
            this.group_command = new GroupCommand(fr);

            this.flag_ready1 = false;
            this.buttonStart1.Text = "开始";
            this.buttonStart1.Enabled = false;

            this.flag_ready2 = false;
            this.buttonStart2.Text = "开始";
            this.buttonStart2.Enabled = false;

            Draw_Init();

            action1 = new System.Timers.Timer();
            action1.Interval = 200;
            action1.Enabled = Enabled;
            action1.Stop();
            action1.Elapsed += new ElapsedEventHandler(action1_tick);

            action2 = new System.Timers.Timer();
            action2.Interval = 150;
            action2.Enabled = Enabled;
            action2.Stop();
            action2.Elapsed += new ElapsedEventHandler(action2_tick);
        }

        private void Delay_ms(int mm)
        {
            DateTime current = DateTime.Now;

            while (current.AddMilliseconds(mm) > DateTime.Now)
            {
                Application.DoEvents();
            }
            return;
        }

        private void ShowDegree()
        {
            textBoxShow.Text = "舵机2：" + degree_c.ToString("f1") + "° " + 
                "舵机3：" + degree_b.ToString("f1") + "° " + "舵机4：" + degree_a.ToString("f1") + "°";
        }

        private void RoundCalculate1()     //画圆时l_x为较长值时
        {
            double a, b, c;
            l_x = (l - r) * Math.Cos(radian_d) + Math.Sqrt(r * r - Math.Sin(radian_d) * Math.Sin(radian_d) * (l - r) * (l - r));
            radian_a = Math.Acos((l_x * l_x + l_c * l_c - 2 * l_x * l_c * Math.Cos(radian_c - radian_d) - l_a * l_a - l_b * l_b) / (2 * l_a * l_b));

            a = l_b * Math.Cos(radian_d - radian_c) + l_a * Math.Cos(radian_a - radian_c + radian_d);
            b = l_b * Math.Sin(radian_c - radian_d) + l_a * Math.Sin(radian_c - radian_d - radian_a);
            c = l_x - l_c * Math.Cos(radian_c - radian_d);

            radian_b = Math.Acos(c / Math.Sqrt(a * a + b * b)) + Math.Atan(b / a);
        }

        private void RoundCalculate2()     //画圆时l_x为较短值时
        {
            l_x = (l - r) * Math.Cos(radian_c) - Math.Sqrt(r * r - Math.Sin(radian_c) * Math.Sin(radian_c) * (l - r) * (l - r));
            radian_a = Math.Acos((l_b * l_b + (l_x - l_c) * (l_x - l_c) - l_a * l_a) / (2 * l_b * (l_x - l_c))) 
                       + Math.Acos((l_a * l_a + (l_x - l_c) * (l_x - l_c) - l_b * l_b) / (2 * l_a * (l_x - l_c)));
            radian_b = Math.Acos((l_b * l_b + (l_x - l_c) * (l_x - l_c) - l_a * l_a) / (2 * l_b * (l_x - l_c)));
        }

        private void RoundCalculate3()      //画圆，已知已知radian_a和radian_c ，求radian_b
        {
            double a, b, c;
            a = 2 * (l - r) * l_a * Math.Cos(radian_a - radian_c) + 
                2 * (l - r) * l_b * Math.Cos(radian_c) - 2 * l_a * l_c * Math.Cos(radian_a) - 2 * l_b * l_c;
            b = 2 * (l - r) * l_a * Math.Sin(radian_a - radian_c) - 2 * (l - r) * l_b * Math.Sin(radian_c) - 2 * l_a * l_c * Math.Sin(radian_a);
            c = l * l - 2 * l * r - 2 * (l - r) * l_c * Math.Cos(radian_c) + 
                l_a * l_a + l_b * l_b + l_c * l_c + 2 * l_a * l_b * Math.Cos(radian_a);
            radian_b = Math.Acos(c / Math.Sqrt(a * a + b * b)) + Math.Atan(b / a);
        }

        private void LineCalculate1()
        {
            double a;
            double b;
            radian_a = Math.Acos((l_x * l_x + l_c * l_c - 2 * l_x * l_c * Math.Cos(radian_c) - l_a * l_a - l_b * l_b) / (2 * l_a * l_b));
            a = l_b * Math.Cos(radian_c) + l_a * Math.Cos(radian_a + radian_c);
            b = l_b * Math.Sin(radian_c) + l_a * Math.Sin(radian_a + radian_c);
            radian_b = Math.Atan(b / a) + Math.Acos((l_x - l_c * Math.Cos(radian_c)) / Math.Sqrt(a * a + b * b));
        }   //画直线，有问题，已知radian_c 和 x 求 radian_a，radian_b

        private void LineCalculate2()     //画直线 最长为4.23cm ，已知radian_b 和radian_c ，求radian_a
        {
            radian_a = radian_b - radian_c - Math.Asin((l_c * Math.Sin(radian_c) - l_b * Math.Sin(radian_b - radian_c)) / l_a);
        }

        private void LineCalculate3()    //画直线 最长为8.78cm 已知radian_a和radian_c ，求radian_b
        {
            double a,b;
            a = l_a * Math.Cos(radian_a - radian_c) + l_b * Math.Cos(radian_c);
            b = l_a * Math.Sin(radian_a - radian_c) - l_b * Math.Sin(radian_c);
            radian_b = Math.Asin((l_c * Math.Sin(radian_c)) / Math.Sqrt(a * a + b * b)) - Math.Atan(b / a);
        }

        private void buttonExit_Click(object sender, EventArgs e)
        {
            this.Hide();
            this.Dispose();
        }

        private void buttonReady1_Click(object sender, EventArgs e)
        {
            group_command.Start_T("01", "C4", "00", "20", "02", "2C", "00", "20", "02", "2C", "00", "20", "02", "2C", "00", "20");

            flag_ready1 = true;
            buttonStart1.Enabled = true;
            buttonReady2.Enabled = false;
            buttonStart2.Enabled = false;
            buttonStart1.Text = "开始";
            buttonStart2.Text = "开始";
            //清除节点  00是位置时间  01是连续轨迹
            group_command.ClearBuffer("02", "00");
            group_command.ClearBuffer("03", "00");
            group_command.ClearBuffer("04", "00");

            /////////////
            /*
            while (true)
            {
                c_count++;
                point_count++;

                radian_c = c_count * Math.PI / 180;
                //radian_d = radian_c * degree_d_max / degree_c_max;
                radian_a = radian_c * 9 / 7;
                RoundCalculate3();
                degree_a = radian_a * 180 / Math.PI;
                degree_b = radian_b * 180 / Math.PI;
                degree_c = c_count;
                del_a = (int)Math.Round(degree_a * 3.41);
                del_b = (int)Math.Round(degree_b * 3.41);
                del_c = (int)Math.Round(degree_c * 3.41);
                a_h_byte = (byte)((middle + del_a) >> 8);
                a_l_byte = (byte)((middle + del_a) & 0x0ff);
                b_h_byte = (byte)((middle - del_b) >> 8);
                b_l_byte = (byte)((middle - del_b) & 0x0ff);
                c_h_byte = (byte)((middle + del_c) >> 8);
                c_l_byte = (byte)((middle + del_c) & 0x0ff);

                if (a_h_byte >= 16)
                    a_h_string = Convert.ToString(a_h_byte, 16);
                else
                    a_h_string = "0" + Convert.ToString(a_h_byte, 16);

                if (a_l_byte >= 16)
                    a_l_string = Convert.ToString(a_l_byte, 16);
                else
                    a_l_string = "0" + Convert.ToString(a_l_byte, 16);

                if (b_h_byte >= 16)
                    b_h_string = Convert.ToString(b_h_byte, 16);
                else
                    b_h_string = "0" + Convert.ToString(b_h_byte, 16);

                if (b_l_byte >= 16)
                    b_l_string = Convert.ToString(b_l_byte, 16);
                else
                    b_l_string = "0" + Convert.ToString(b_l_byte, 16);

                if (c_h_byte >= 16)
                    c_h_string = Convert.ToString(c_h_byte, 16);
                else
                    c_h_string = "0" + Convert.ToString(c_h_byte, 16);

                if (c_l_byte >= 16)
                    c_l_string = Convert.ToString(c_l_byte, 16);
                else
                    c_l_string = "0" + Convert.ToString(c_l_byte, 16);

                if ((radian_c * 180 / Math.PI) > degree_c_max)
                {
                    break;
                }
            }
            */
            ////////////
        }

        private void buttonStart1_Click(object sender, EventArgs e)
        {
            if (flag_ready1)
            {
                if (buttonStart1.Text == "开始" || buttonStart1.Text == "继续")   //显示为“开始”   开始运动
                {
                    if (buttonStart1.Text == "开始")
                    {
                        this.frmain.unstop_counter_arm = 0;
                        this.frmain.group_counter_arm = 0;
                        Draw_Init();
                        group_command.ClearBuffer("02", "00");
                        group_command.ClearBuffer("03", "00");
                        group_command.ClearBuffer("04", "00");
                        //c_count = 20;
                    }

                    buttonStart1.Text = "停止";
                    buttonReady1.Enabled = false;
                    buttonReady2.Enabled = false;

                    group_command.Move_Curve();
                    action1.Start();

                }
                else if (buttonStart1.Text == "停止")           //显示为“停止”    停止运动
                {
                    //Break();
                    buttonStart1.Text = "继续";
                    buttonReady1.Enabled = true;
                    buttonReady2.Enabled = true;
                    ShowDegree();

                    action1.Stop();
                }
            }
        }

        private void action1_tick(object sender, ElapsedEventArgs e) 
        {
            if (this.frmain.group_counter_arm == 0)
            {
                this.frmain.group_counter_arm = 1;
            }
            else if (this.frmain.group_counter_arm == 1)
            {
                if (this.frmain.unstop_counter_arm == 0)
                {
                    c_count = c_count + 7;
                    point_count++;

                    // RoundCalculate1() a + b + c +     RoundCalculate3() a + b - c +
                    radian_c = c_count * Math.PI / 180;
                    //radian_d = radian_c * degree_d_max / degree_c_max;
                    radian_a = radian_c * 9 / 7;
                    RoundCalculate3();
                    degree_a = radian_a * 180 / Math.PI;
                    degree_b = radian_b * 180 / Math.PI;
                    degree_c = c_count;
                    del_a = (int)Math.Round(degree_a * 3.41);
                    del_b = (int)Math.Round(degree_b * 3.41);
                    del_c = (int)Math.Round(degree_c * 3.41);
                    a_h_byte = (byte)((middle + del_a) >> 8);
                    a_l_byte = (byte)((middle + del_a) & 0x0ff);
                    b_h_byte = (byte)((middle - del_b) >> 8);
                    b_l_byte = (byte)((middle - del_b) & 0x0ff);
                    c_h_byte = (byte)((middle + del_c) >> 8);
                    c_l_byte = (byte)((middle + del_c) & 0x0ff);

                    if (a_h_byte >= 16)
                        a_h_string = Convert.ToString(a_h_byte, 16);
                    else
                        a_h_string = "0" + Convert.ToString(a_h_byte, 16);

                    if (a_l_byte >= 16)
                        a_l_string = Convert.ToString(a_l_byte, 16);
                    else
                        a_l_string = "0" + Convert.ToString(a_l_byte, 16);

                    if (b_h_byte >= 16)
                        b_h_string = Convert.ToString(b_h_byte, 16);
                    else
                        b_h_string = "0" + Convert.ToString(b_h_byte, 16);

                    if (b_l_byte >= 16)
                        b_l_string = Convert.ToString(b_l_byte, 16);
                    else
                        b_l_string = "0" + Convert.ToString(b_l_byte, 16);

                    if (c_h_byte >= 16)
                        c_h_string = Convert.ToString(c_h_byte, 16);
                    else
                        c_h_string = "0" + Convert.ToString(c_h_byte, 16);

                    if (c_l_byte >= 16)
                        c_l_string = Convert.ToString(c_l_byte, 16);
                    else
                        c_l_string = "0" + Convert.ToString(c_l_byte, 16);

                    group_command.AddPoint_Curve(c_h_string, c_l_string, "02", "60", b_h_string, b_l_string, "02", "60", a_h_string, a_l_string, "02", "60");

                    if (degree_c > (degree_c_max - 2))
                    {
                        this.frmain.unstop_counter_arm = 1;
                    }
                    if (point_count >= 4)
                    {
                        this.frmain.group_counter_arm = 2;
                        point_count = 0;
                    }
                }
               
            }
            else if (this.frmain.group_counter_arm == 2)
            {
                this.frmain.group_counter_arm = 3;
                group_command.Move_Curve();
                //ShowDegree();
            }
            else if (this.frmain.group_counter_arm == 3)
            {
                this.frmain.group_counter_arm = 3;
                group_command.BufferLeft_Curve("02");
            }

        }

        private void buttonReady2_Click(object sender, EventArgs e)
        {
            group_command.Start_T("01", "C4", "00", "20", "02", "2C", "00", "20", "02", "2C", "00", "20", "02", "2C", "00", "20");

            flag_ready2 = true;
            buttonStart2.Enabled = true;
            buttonReady1.Enabled = false;
            buttonStart1.Enabled = false;
            buttonStart1.Text = "开始";
            buttonStart2.Text = "开始";
            //清除节点  00是位置时间  01是连续轨迹
            group_command.ClearBuffer("02", "00");
            group_command.ClearBuffer("03", "00");
            group_command.ClearBuffer("04", "00");
        }

        private void buttonStart2_Click(object sender, EventArgs e)
        {
            if (flag_ready2)
            {
                if (buttonStart2.Text == "开始" || buttonStart2.Text == "继续")   //显示为“开始”   开始运动
                {
                    if (buttonStart2.Text == "开始")
                    {
                        this.frmain.unstop_counter_arm = 0;
                        this.frmain.group_counter_arm = 0;
                        Draw_Init();
                        group_command.ClearBuffer("02", "00");
                        group_command.ClearBuffer("03", "00");
                        group_command.ClearBuffer("04", "00");
                    }

                    buttonStart2.Text = "停止";
                    buttonReady1.Enabled = false;
                    buttonReady2.Enabled = false;

                    group_command.Move_Curve();
                    action2.Start();

                }
                else if (buttonStart2.Text == "停止")           //显示为“停止”    停止运动
                {
                    //group_command.Break();
                    buttonStart2.Text = "继续";
                    buttonReady2.Enabled = true;
                    buttonReady1.Enabled = true;

                    action2.Stop();
                }
            }
        }

        private void action2_tick(object sender, ElapsedEventArgs e)
        {
            if (this.frmain.group_counter_arm == 0)
            {
                this.frmain.group_counter_arm = 1;
            }
            else if (this.frmain.group_counter_arm == 1)
            {
                if (this.frmain.unstop_counter_arm == 0)
                {
                    c_count = c_count + 7;
                    point_count++;

                    // a + b + c + 
                    radian_c = c_count * Math.PI / 180;
                    radian_a = radian_c * 9 / 7;
                    LineCalculate3();
                    degree_a = radian_a * 180 / Math.PI;
                    degree_b = radian_b * 180 / Math.PI;
                    degree_c = c_count;
                    del_a = (int)Math.Round(degree_a * 3.41);
                    del_b = (int)Math.Round(degree_b * 3.41);
                    del_c = (int)Math.Round(degree_c * 3.41);
                    a_h_byte = (byte)((middle + del_a) >> 8);
                    a_l_byte = (byte)((middle + del_a) & 0x0ff);
                    b_h_byte = (byte)((middle + del_b) >> 8);
                    b_l_byte = (byte)((middle + del_b) & 0x0ff);
                    c_h_byte = (byte)((middle + del_c) >> 8);
                    c_l_byte = (byte)((middle + del_c) & 0x0ff);

                    if (a_h_byte >= 16)
                        a_h_string = Convert.ToString(a_h_byte, 16);
                    else
                        a_h_string = "0" + Convert.ToString(a_h_byte, 16);

                    if (a_l_byte >= 16)
                        a_l_string = Convert.ToString(a_l_byte, 16);
                    else
                        a_l_string = "0" + Convert.ToString(a_l_byte, 16);

                    if (b_h_byte >= 16)
                        b_h_string = Convert.ToString(b_h_byte, 16);
                    else
                        b_h_string = "0" + Convert.ToString(b_h_byte, 16);

                    if (b_l_byte >= 16)
                        b_l_string = Convert.ToString(b_l_byte, 16);
                    else
                        b_l_string = "0" + Convert.ToString(b_l_byte, 16);

                    if (c_h_byte >= 16)
                        c_h_string = Convert.ToString(c_h_byte, 16);
                    else
                        c_h_string = "0" + Convert.ToString(c_h_byte, 16);

                    if (c_l_byte >= 16)
                        c_l_string = Convert.ToString(c_l_byte, 16);
                    else
                        c_l_string = "0" + Convert.ToString(c_l_byte, 16);

                    group_command.AddPoint_Curve(c_h_string, c_l_string, "02", "00", b_h_string, b_l_string, "02", "00", a_h_string, a_l_string, "02", "00");

                    if (Convert.ToDouble(c_count) > degree_c_max)
                    {
                        this.frmain.unstop_counter_arm = 1;
                    }
                    if (point_count >= 4)
                    {
                        this.frmain.group_counter_arm = 2;
                        point_count = 0;
                    }
                }
            }
            else if (this.frmain.group_counter_arm == 2)
            {
                this.frmain.group_counter_arm = 3;
                group_command.Move_Curve();
            }
            else if (this.frmain.group_counter_arm == 3)
            {
                this.frmain.group_counter_arm = 3;
                group_command.BufferLeft_Curve("02");
            }
        }
    }
}
